#include "ColorUtil.h"

#include <windows.h>

bool RGB24_TO_YUV420(BYTE* RgbBuf,UINT nWidth,UINT nHeight,BYTE* yuvBuf,unsigned long *len)
{
    int i, j;
    unsigned char*bufY, *bufU, *bufV, *bufRGB,*bufYuv;
    memset(yuvBuf,0,(unsigned int )*len);
    bufY = yuvBuf;
    bufV = yuvBuf + nWidth * nHeight;
    bufU = bufV + (nWidth * nHeight* 1/4);
    *len = 0;
    unsigned char y, u, v, r, g, b,testu,testv;
    unsigned int ylen = nWidth * nHeight;
    unsigned int ulen = (nWidth * nHeight)/4;
    unsigned int vlen = (nWidth * nHeight)/4;
    for (j = 0; j<nHeight;j++)
    {
        bufRGB = RgbBuf + nWidth * (nHeight - 1 - j) * 3 ;
        for (i = 0;i<nWidth;i++)
        {
            int pos = nWidth * i + j;
            r = *(bufRGB++);
            g = *(bufRGB++);
            b = *(bufRGB++);

            y = (unsigned char)( ( 66 * r + 129 * g + 25 * b + 128) >> 8) + 16 ;
            u = (unsigned char)( ( -38 * r - 74 * g + 112 * b + 128) >> 8) + 128 ;
            v = (unsigned char)( ( 112 * r - 94 * g - 18 * b + 128) >> 8) + 128 ;
            *(bufY++) = max( 0, min(y, 255 ));

            if (j%2==0&&i%2 ==0)
            {
                if (u>255)
                {
                    u=255;
                }
                if (u<0)
                {
                    u = 0;
                }
                *(bufU++) =u;
            } else {
                if (i%2==0)
                {
                    if (v>255)
                    {
                        v = 255;
                    }
                    if (v<0)
                    {
                        v = 0;
                    }
                    *(bufV++) =v;

                }
            }

        }

    }
    *len = nWidth * nHeight+(nWidth * nHeight)/2;
    return true;
}


void YUV420_TO_RGB24 (BYTE *buffer,BYTE *pbR,BYTE *pbG,BYTE *pbB)
{

    const int bmp_wd =352;
    const int bmp_ht=288;
    unsigned char *bufy , *bufu , *bufv;

    short int r, c,R, G, B, y, u, v;
    byte* yuvBuf;
    yuvBuf=buffer;

    bufy = yuvBuf;
    bufu = yuvBuf + bmp_wd * bmp_ht;
    bufv = yuvBuf + bmp_wd * bmp_ht * 3/2;

    long iIndex=0;

    for (r = 0; r < bmp_ht; r++) {

        for (c = 0; c < bmp_wd; c++) {
            y = bufy [c];
            u = bufu [c >> 1] - 128;

            v = bufv [c >> 1] - 128;

            R = y + 1.375 * v;
            G = y - 0.34375 * u - 0.703125 * v;
            B = y + 1.734375 * u;
            R = max (0, min (255, R));
            G = max (0, min (255, G));
            B = max (0, min (255, B));

            pbR[iIndex+c]=R;
            pbG[iIndex+c]=G;
            pbB[iIndex+c]=B;
        }


        bufy += bmp_wd;
        bufu += bmp_wd/2;
        bufv += bmp_wd/2;
        iIndex+=bmp_wd;
    }
}